﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Text;

namespace SharpSoft.Geometries
{
    /// <summary>
    /// 平面坐标的运动管理
    /// </summary>
    public class PointMotionManager : IDisposable
    {
        public PointMotionManager()
        {
            this.CreateAffirm = TimeSpan.FromMilliseconds(200);
            this.DisappearAffirm = TimeSpan.FromMilliseconds(500);
            stopwatch = new Stopwatch();
            stopwatch.Start();
        }
        public PointMotionManager(TimeSpan createAffirm, TimeSpan disappearAffirm) : this()
        {
            this.CreateAffirm = createAffirm;
            this.DisappearAffirm = disappearAffirm;
        }
        private Stopwatch stopwatch;//计时器
        private TimeSpan lastFrameTime;//记录上一帧的时间
        private readonly List<MotionPoint> curFrame = new List<MotionPoint>();//记录当前帧的数据

        /// <summary>
        /// 管理的点最大的生存时长，超过这个时间，无论点是何状态都将被移除
        /// </summary>
        public TimeSpan MaxLifeTime { get; set; } = TimeSpan.FromDays(1);
        /// <summary>
        /// 获取当前帧
        /// </summary>
        /// <returns></returns>
        public MotionPoint[] GetCurrentFrame()
        {
            lock (curFrameLocker)
            {
                return curFrame.ToArray();
            }
        }
        /// <summary>
        /// 清理当前帧数据（一般在所有点都清空时调用）
        /// </summary>
        public void ClearCurrentFrame()
        {
            lock (curFrameLocker)
            {
                curId = uint.MinValue;
                curFrame.Clear();
            }
        }
        private readonly object curFrameLocker = new object();

        private long frameIndex = 0;//当前帧索引
        private uint curId = 0;//当前的点Id
        private TimeSpan frameNow;//当前帧时间
        /// <summary>
        /// 指定一个时间段(默认为200ms)，当一个坐标点持续该时间未出现时认定其已消失
        /// </summary>
        public TimeSpan DisappearAffirm { get; set; }
        /// <summary>
        /// 指定一个时间段(默认为500ms)，当一个坐标点持续该时间出现时认定其已创建
        /// </summary>
        public TimeSpan CreateAffirm { get; set; }
        /// <summary>
        /// 坐标允许的最大速度(单位距离/秒)
        /// </summary>
        public double MaxSpeed { get; set; } = 1000;

        /// <summary>
        /// 输入新帧
        /// </summary>
        /// <param name="points"></param>
        public void NewFrame(Point2D[] points)
        {
            this.frameNow = stopwatch.Elapsed;
            if (frameIndex >= long.MaxValue - 1)
            {
                frameIndex = 0;
            }
            frameIndex++;//递增帧索引

            this.CompareFrame(points);

            this.lastFrameTime = this.frameNow;
        }

        /// <summary>
        /// 将新数据与当前帧进行比较
        /// </summary>
        /// <param name="points"></param>
        private void CompareFrame(Point2D[] points)
        {
            List<MotionPoint> added = new List<MotionPoint>();
            List<MotionPoint> moved = new List<MotionPoint>();
            List<MotionPoint> removed = new List<MotionPoint>();
            List<MotionPoint> unchanged = new List<MotionPoint>();
            if (points != null)
            {
                foreach (var p in points)
                {
                    var mp = FindNearPoint(p);
                    if (mp == null)
                    {//找不到关联点，创建新的疑似点
                        mp = this.createMotionPoint(p);
                        lock (this.curFrameLocker)
                        {
                            curFrame.Add(mp);
                        }
                    }
                    else
                    {//找到了关联点 更新其位置
                        /**************************
                        TODO:
                        有一种情况，一个旧点被多个新点匹配到。
                        目前解决方案为排除已匹配过的点。
                        后期考虑使用“全局最优”方案来解决，保证在全局下新点与旧点之间的距离匹配始终是“全局最优”的。
                        20191119
                        *********************/
                        mp.LastFrameIndex = this.frameIndex;
                        mp.LastTime = frameNow;
                        bool ismoved = (mp.Position != p);
                        mp.Update(p, frameNow.TotalMilliseconds);
                        if (mp.Status == MotionPointStatus.SuspectedCreate)
                        {//疑似要创建的点
                            if (frameNow - mp.CreateTime >= this.CreateAffirm)
                            {//该点已持续出现
                                mp.Status = MotionPointStatus.Exists;
                                //触发Added事件
                                added.Add(mp);
                            }
                        }
                        else if (mp.Status == MotionPointStatus.SuspectedRemove)
                        {//疑似要移除的点
                            if (frameNow - mp.LastTime <= this.DisappearAffirm)
                            {//该点在指定的时间内重新出现
                                mp.Status = MotionPointStatus.Exists;
                            }
                        }
                        else
                        {//触发Moved事件
                            if (ismoved)
                            {
                                moved.Add(mp);
                            }
                        }
                    }
                }
            }
            List<MotionPoint> removeTemp = new List<MotionPoint>(0);//缓存要移除的点
            lock (curFrameLocker)
            {
                foreach (var item in this.curFrame)
                {
                    if (item.LastFrameIndex != this.frameIndex)
                    {//未匹配到
                        if (item.Status == MotionPointStatus.Exists)
                        {//已经存在的点标记为疑似移除
                            item.Status = MotionPointStatus.SuspectedRemove;
                            unchanged.Add(item);
                        }
                        else if (item.Status == MotionPointStatus.SuspectedRemove)
                        { //检测移除超时
                            if (frameNow - item.LastTime >= DisappearAffirm)
                            {//超出指定时间未出现，认定其已移除
                                removeTemp.Add(item);
                            }
                        }
                        else if (item.Status == MotionPointStatus.SuspectedCreate)
                        {
                            if (frameNow - item.LastTime >= CreateAffirm)
                            {//超出指定时间再未出现，认定其是噪点，移除临时点
                                removeTemp.Add(item);
                            }
                        }

                    }
                    if (frameNow - item.CreateTime >= this.MaxLifeTime)
                    {//超出了生存期限,强行移除
                        if (!removeTemp.Contains(item))
                        {
                            removeTemp.Add(item);
                        }
                    }

                }

                //移除要移除的点
                foreach (var item in removeTemp)
                {
                    this.curFrame.Remove(item);
                }
            }
            foreach (var item in removeTemp)
            {//通知点已移除 
                    removed.Add(item);  
            }
            removeTemp.Clear();

            MotionPointEventArgs e = new MotionPointEventArgs()
            {
                Added = added.ToArray(),
                Removed = removed.ToArray(),
                Moved = moved.ToArray(),
                UnChanged = unchanged.ToArray(),
            };
            if (e.Added.Length > 0 || e.Removed.Length > 0 || e.Moved.Length > 0 || e.UnChanged.Length > 0)
            {//有任意变化则触发帧事件
                this.OnActiveFrame(e);
            }

        }
        /// <summary>
        /// 查找距离最近的点 
        /// </summary>
        /// <param name="p"></param>
        /// <param name="now"></param>
        /// <returns></returns>
        private MotionPoint FindNearPoint(Point2D p)
        {
            double? min = null;
            MotionPoint minPo = null;
            foreach (var item in this.curFrame)
            {
                if (item.LastFrameIndex == this.frameIndex)
                {//跳过当前帧已匹配过的点
                    continue;
                }
                var v = p - item.Position;
                if (!min.HasValue)
                {
                    min = v.Length;
                    minPo = item;
                }
                else
                {

                    if (v.Length < min.Value)
                    {
                        min = v.Length;
                        minPo = item;
                    }
                }
            }

            if (minPo != null)
            {
                var maxv = this.MaxSpeed * (frameNow - minPo.CreateTime).TotalSeconds;
                if (min.GetValueOrDefault() >= maxv)
                {//距离最近的点都超出了最大速度允许的范围，当作找不到匹配点
                    return null;
                }
                minPo.LastFrameIndex = this.frameIndex;
            }

            return minPo;
        }

        public void Dispose()
        {
            stopwatch.Stop();
        }
        /// <summary>
        /// 根据当前上下文创建运动点
        /// </summary>
        /// <param name="p"></param>
        /// <returns></returns>
        private MotionPoint createMotionPoint(Point2D p)
        {
            var mp = new MotionPoint(p) { Id = curId, CreateTime = frameNow, LastTime = frameNow, LastFrameIndex = frameIndex, Status = MotionPointStatus.SuspectedCreate };
            curId++;
            if (curId >= uint.MaxValue - 1)
            {
                curId = 0;
            }
            return mp;
        }

        /// <summary>
        /// 新的帧被激活
        /// </summary>
        public event MotionPointEventHandler ActiveFrame;


        protected void OnActiveFrame(MotionPointEventArgs e)
        {
            this.ActiveFrame?.Invoke(this, e);
        }
    }
    /// <summary>
    /// 运动点事件处理程序
    /// </summary>
    /// <param name="sender"></param>
    /// <param name="e"></param>
    public delegate void MotionPointEventHandler(object sender, MotionPointEventArgs e);
    /// <summary>
    /// 运动点事件参数
    /// </summary>
    public class MotionPointEventArgs
    {
        public MotionPointEventArgs()
        {
        }
        /// <summary>
        /// 未变化
        /// </summary>
        public MotionPoint[] UnChanged { get; internal set; }
        /// <summary>
        /// 新增的
        /// </summary>
        public MotionPoint[] Added { get; internal set; }
        /// <summary>
        /// 移除的
        /// </summary>
        public MotionPoint[] Removed { get; internal set; }
        /// <summary>
        /// 移动的
        /// </summary>
        public MotionPoint[] Moved { get; set; }
    }

    /// <summary>
    /// 描述一个运动的坐标点
    /// </summary>
    public class MotionPoint
    {
        public MotionPoint(Point2D initPos)
        {
            this.filter = new Kalman.KalmanFilter();
            this.filter.Reset(initPos);
            this.Position = initPos;
        }
        Kalman.KalmanFilter filter;
        public uint Id { get; set; }
        /// <summary>
        /// 坐标位置
        /// </summary>
        public Point2D Position { get; private set; }
        /// <summary>
        /// 点第一次出现的时间
        /// </summary>
        public TimeSpan CreateTime { get; internal set; }

        public void Update(Point2D newpos, double time)
        {
            this.Position = this.filter.Update(newpos, time);
        }

        /// <summary>
        /// 上一次出现(被匹配到)的帧索引
        /// </summary>
        public long LastFrameIndex { get; internal set; } = -1;
        /// <summary>
        /// 点上一次出现的时间
        /// </summary>
        public TimeSpan LastTime { get; internal set; }
        /// <summary>
        /// 该运动点上一次移动的矢量
        /// </summary>
        public Vector2D LastVector { get; internal set; }
        /// <summary>
        /// 点的状态
        /// </summary>
        public MotionPointStatus Status { get; internal set; }
    }

    public enum MotionPointStatus
    {
        /// <summary>
        /// 疑似要创建
        /// </summary>
        SuspectedCreate,
        /// <summary>
        /// 已存在
        /// </summary>
        Exists,
        /// <summary>
        /// 疑似要移除
        /// </summary>
        SuspectedRemove

    }
}
